#pragma once

#include "btBulletDynamicsCommon.h"
#include "RigidBody.h"

///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets.
class RigidBodyMotionState : public btMotionState
{
public:
	btTransform m_graphicsWorldTrans;
	btTransform	m_centerOfMassOffset;
	btTransform m_startWorldTrans;
	RigidBody* m_rigidBody;

	RigidBodyMotionState(RigidBody* body, const btTransform& startTrans = btTransform::getIdentity(),const btTransform& centerOfMassOffset = btTransform::getIdentity())
		: m_graphicsWorldTrans(startTrans),
		m_centerOfMassOffset(centerOfMassOffset),
		m_startWorldTrans(startTrans),
		m_rigidBody(body)
	{
	}

	///synchronizes world transform from user to physics
	virtual void	getWorldTransform(btTransform& centerOfMassWorldTrans ) const 
	{
		Maths::Quaternion& orient = m_rigidBody->getRotation().getObject();
		Maths::Vector3D& posit = m_rigidBody->getPosition().getObject();

		btQuaternion rot(orient.X(), orient.Y(), orient.Z(), orient.W());
		btVector3 pos(posit.X(), posit.Y(), posit.Z());
		const btTransform trans(rot, pos);

		centerOfMassWorldTrans = 	m_centerOfMassOffset.inverse() * trans;
	}

	///synchronizes world transform from physics to user
	///Bullet only calls the update of worldtransform for active objects
	virtual void	setWorldTransform(const btTransform& centerOfMassWorldTrans)
	{
		btVector3 position;
		m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ;
		position = m_graphicsWorldTrans.getOrigin();
		m_rigidBody->getPosition() = Maths::Vector3D(position.x(), position.y(), position.z());
	}
};